#include <Wire.h>
#include <ADXL345.h>

//const float alpha = 0.5;

//double fXg = 0;
//double fYg = 0;
//double fZg = 0;

ADXL345 acc;

void setup()
{
    double Xg, Yg, Zg;
    acc.begin();
    Serial.begin(9600);
    delay(100);
	
    //acc.read();
	
	//Xg = acc.getGX();
	//Yg = acc.getGY();
	//Zg = acc.getGZ();
	
	//acc.setZeroG(Xg, Yg, Zg);
}

void loop()
{
    double pitch, roll, Xg, Yg, Zg;
    //acc.read(&Xg, &Yg, &Zg);
    acc.read();
	
	Xg = acc.getGX();
	Yg = acc.getGY();
	Zg = acc.getGZ();
	
    //Serial.print(Xg);
    //Serial.print(":");
    //Serial.print(Yg);
    //Serial.print(":");
    //Serial.println(Zg);
 	

    //Low Pass Filter
    //fXg = Xg * alpha + (fXg * (1.0 - alpha));
    //fYg = Yg * alpha + (fYg * (1.0 - alpha));
    //fZg = Zg * alpha + (fZg * (1.0 - alpha));
	
    acc.processLPFilter();
 
    //Roll & Pitch Equations
    //roll  = (atan2(-fYg, fZg) * 180.0) / M_PI;
    //pitch = (atan2(fXg, sqrt(fYg * fYg + fZg * fZg)) * 180.0) / M_PI;
 
    roll  = acc.getRoll();
    pitch = acc.getPitch(); 	
 
    Serial.print(pitch);
    Serial.print(":");
    Serial.println(roll);
 
    delay(10);
}

